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identifying the locations and motions of other vessels, as well 
as the locations of stationary objects that represent navigation 
hazards. By applying a computational method that uses a 
maritime navigation algorithm for avoiding hazards and 
obeying COLREGS using Velocity Obstacles to the data 
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gational procedures defined by international agreement. The 
systems and methods have been successfully demonstrated 
on water with radar and stereo cameras as the perception 
sensors, and integrated with a higher level planner for trailing 
a maneuvering target. 
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TARGET TRAILING WITH SAFE 
NAVIGATION WITH COLREGS FOR 
MARITIME AUTONOMOUS SURFACE 
VEHICLES 

CROSS-REFERENCE TO RELATED 
APPLICATIONS 

This application claims priority to and the benefit of U.S. 
provisional patent application Ser. No. 61/474,867 filed Apr. 
13, 2011, and claims priority to and the benefit ofU.S. pro- 
visional patent application Ser. No. 61/499,819 filed Jun. 22, 
2011, each of which applications is incorporated herein by 
reference in its entirety. 

STATEMENT REGARDING FEDERALLY 
FUNDED RESEARCH OR DEVELOPMENT 

The invention described herein was made in the perfor- 
mance of work under a NASA contract, and is subject to the 
provisions of Public Law 96-517 (35 USC 202) in which the 
Contractor has elected to retain title. 

THE NAMES OF THE PARTIES TO A JOINT 
RESEARCH AGREEMENT 

The inventions claimed in the present application were 
made as a result of activities undertaken within the scope of a 
Joint Research Agreement between the Jet Propulsion Labo- 
ratory (JPL) operated by the California Institute of Technol- 
ogy for NASA and the U.S. Naval Surface Warfare Center, 
Norfolk, Va. under JPL Task Plan No. 90-8952 entered into in 
2004, and a Joint Research Agreement between JPL and the 
U.S. Navy SPAWAR Systems Center San Diego, San Diego, 
Calif, under JPL Task Plan No. 8 1 -9996 entered into in 2005. 

FIELD OF THE INVENTION 

The invention relates to systems and methods for operating 
autonomous vehicles in general and particularly to systems 
and methods for operating waterborne autonomous vehicles. 

BACKGROUND OF THE INVENTION 

Operation of vessels in a maritime setting involves adher- 
ence to standardized navigation rules that are intended to 
avoid collisions or other dangerous navigation conditions. 
These standardized navigation rules (the International Regu- 
lations for Preventing Collisions at Sea, known as COL- 
REGS) are well known and are generally adhered to by all 
vessels, from small sailing vessels to commercial vessels 
such as cruise liners and shipping vessels such as container 
ships and tankers, as well as military vessels when operating 
under conventional operating conditions. 

In recent years, significant technological advancements 
have increased on-board capabilities of Unmanned Surface 
Vehicles (USVs), so that their intended mission scenarios 
now routinely include environments shared with other seago- 
ing traffic. See Program Executive Officer for Littoral and 
Mine Warfare (PEO (LMW)), “The Navy Unmanned Surface 
Vehicle (USV) Master Plan,” U.S. Navy, Tech. Rep., 2007; 
and J. Larson, M. Bruch, and J. Ebken, “Autonomous navi- 
gation and obstacle avoidance for unmanned surface 
vehicles,” in SPIE Proc. 6230: Unmanned Systems Technol- 
ogy VIII, 2006, pp. 17-20. 

In maritime navigation, ships should obey the International 
Regulations for Preventing Collisions at Sea (known as COL- 
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REGS, for Collision Regulations), agreed to by the Interna- 
tional Maritime Oiganization (IMO) in 1972. See U.S. 
Department Of Homeland Security, U.S. Coast Guard, Navi- 
gation Rules. Paradise Cay Publications, 2010. These “rules 
5 of the road” specify the types of maneuvers that should be 
taken in situations where there is a risk of collision. When 
USVs are operated in the vicinity of other vessels, their navi- 
gation algorithms must abide by COLREGS, so that the 
USVs can safely avoid other vessels and the drivers of other 
to vessels can rely on a range of safe behaviors from the USVs. 
A variety of approaches to maritime navigation obeying 
COLREGS has been proposed in the past, such as fuzzy logic 
(see S.-M. Lee, K.-Y. Kwon, and J. Joh, “A Fuzzy Logic for 
Autonomous Navigation of Marine Vehicles Satislying COL- 
15 REG Guidelines,” International Journal of Control, Automa- 
tion, and Systems, vol. 2, no. 2, pp. 171-181, 2004; and L. P. 
Perera, J. P. Carvalho, and C. G. Soares, “Autonomous guid- 
ance and navigation based on the COLREGs rules and regu- 
lations of collision avoidance,” in Proceedings of the Intema- 
20 tional Workshop “Advanced Ship Design for Pollution 
Prevention”, 2009, pp. 205-216.), evolutionary algorithms 
(see J. Colito, “Autonomous Mission Planning and Execution 
for Unmanned Surface Veliicles in Compliance with the 
Marine Rules of the Road,” Master’s thesis. University of 
25 Washington, 2007.), neural networks, hybrids of these algo- 
rithms (see T. Statheros, G. Howells, and K. M. Maier, 
“Autonomous Ship Collision Avoidance Navigation Con- 
cepts, Technologies and Techniques,” Journal of Navigation, 
vol. 61, no. 1, pp. 129-142, 2008.), interval programming (see 
30 M. Benjamin, J. Curcio, and P. Newman, “Navigation of 
Umnaimed Marine Vehicles in Accordance with the Rules of 
the Road,” in Proceedings of the IEEE International Confer- 
ence on Robotics and Automation, 2006.), and 2D grid maps 
(see K. Teo, K. W. Ong, and H. C. Lai, “Obstacle Detection, 
35 Avoidance and Anti Collision for MEREDITH AUV,” in 
OCEANS, MTS/IEEE Biloxi — Marine Technology for Our 
Future: Global and Local Challenges, 2009.). However, these 
previous approaches do not scale well to multiple traffic boats 
and multiple COLREGS rules, especially on robotic plat- 
40 forms with real-time computational requirements. Further- 
more, most results cited in the literature are limited to simu- 
lation, where real world issues such as uncertainties of USV 
motion, computational and communication delays, and noise 
in the perception system are not present. 

45 There is a need for systems and methods for navigating and 

operating autonomous waterborne vessels that provide safe 
operation while accomplishing a desired sailing action. 

SUMMARY OF THE INVENTION 

50 

According to one aspect, the invention features a system 
for navigating an unmanned waterborne surface vehicle 
safely to a desired location. The system comprises one or 
more sensors configured to be mounted on an unmanned 
55 waterborne surface vehicle, the one or more sensors config- 
ured to detect the presence of one or more objects different 
from the unmanned waterborne surface vehicle in the vicinity 
of the one or more sensors, each of the one or more sensors 
having a control terminal configured to allow control of the 
60 sensor, and each of the one or more sensors having an output 
terminal configured to provide a signal representative of the 
presence of the one or more objects different from the 
unmaimed waterborne surface vehicle; and a general purpose 
programmable computer including instruction recorded on a 
65 machine readable memory, the general purpose program- 
mable computer operatively coupled to each of the one or 
more sensors. The general purpose programmable computer 
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when operating under the instructions configured to perform 
the following steps: (i) controlling an operational state of each 
of the one or more sensors, the operational state selected from 
the group of operational states consisting of an on state and an 
off state; (ii) receiving from the one or more sensors one or 
more respective output signals representative of the presence 
of the one or more objects different from the unmanned 
waterborne surface vehicle; (iii) deducing from the received 
one or more respective output signals whether there is present 
the one or more objects different from the unmanned water- 
borne surface vehicle; (iv) if there is present the one or more 
objects different from the unmanned waterborne surface 
vehicle, computing for each such object a location, a velocity 
and a heading; (v) applying a first set of rules configured to 
direct the unmanned waterborne surface vehicle to reach a 
desired location; (vi) applying a second set of rules config- 
ured to direct the unmanned waterborne surface vehicle to 
avoid each of the one or more objects different from the 
unmanned waterborne surface vehicle that are present; (vii) 
generating a command to be issued to the unmanned water- 
borne surface vehicle to operate at a particular velocity and 
along a particular heading, so as to approach the desired 
location while avoiding a collision with each of the one or 
more objects different from the unmanned waterborne sur- 
face vehicle that are present; and (viii) iteratively repeating 
steps (i) through (vii) until the desired location is reached. 

In one embodiment, the system further comprises an 
unmarmed waterborne surface vehicle operatively connected 
to the one or more sensors and to the general purpose pro- 
grammable computer. 

In another embodiment, the object different from the 
unmanned waterborne surface vehicle is a hazardous object. 

In yet another embodiment, the object different from the 
unmanned waterborne surface vehicle is an object of interest 
to be followed. 

In still another embodiment, the desired location is a posi- 
tion in proximity to the object of interest to be followed. 

In a further embodiment, the desired location is updated 
using the most recent position of the object of interest to be 
followed. 

In yet a further embodiment, the desired location is a loca- 
tion defined on a map of the Earth’s surface. 

In an additional embodiment, the general purpose pro- 
grammable computer when operating under the instructions 
configured to perform the step of displaying to a user infor- 
mation that is representative of the location, the velocity and 
the heading for at least one of the one or more objects different 
from the unmanned waterborne surface vehicle deduced as 
being present in step (iii). 

In one more embodiment, the location, the velocity and the 
heading are computed in step (iv) relative to a location, a 
velocity and a heading of the unmanned waterborne surface 
vehicle. 

In still a further embodiment, the general purpose program- 
mable computer includes a user interface that allows a human 
operator to override a command generated in step (viii). 

In a further embodiment, the first set of rules is a strategic 
planner. 

In yet a further embodiment, the second set of rules is a 
local planner. 

In an additional embodiment, the second set of rules 
includes COLREGS rules. 

In one more embodiment, the one or more sensors are 
selected from the group of sensors consisting of radar, sonar, 
lidar, and stereo cameras. 

According to another aspect, the invention relates to a 
method of navigating an unmanned waterborne surface 
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vehicle safely to a desired location. The method comprising 
the steps of: controlling an operational state of each of one or 
more sensors, the operational state selected from the group of 
operational states consisting of an on state and an off state, the 
5 one or more sensors configured to detect the presence of one 
or more objects different from the unmanned waterborne 
surface vehicle in the vicinity of the one or more sensors, each 
of the one or more sensors having a control terminal config- 
ured to allow control of the sensor, and each of the one or 
more sensors having an output terminal configured to provide 
a signal representative of the presence of the one or more 
objects different from the unmanned waterborne surface 
veliicle; and using a general purpose programmable computer 
including instruction recorded on a machine readable 
memory to perform the following steps: receiving from the 
one or more sensors one or more respective output signals 
representative of the presence of the one or more objects 
different from the unmanned waterborne surface vehicle; 
20 deducing from the received one or more respective output 
signals whether there is present the one or more objects dif- 
ferent from the unmanned waterborne surface vehicle; if there 
is present the one or more objects different from the 
unmamied waterborne surface vehicle, computing for each 
25 such object a location, a velocity and a heading; applying a 
first set of rules configured to direct the unmanned waterborne 
surface veliicle to reach a desired location; applying a second 
set of rules configured to direct the unmanned waterborne 
surface vehicle to avoid each of the one or more objects 
30 different from the unmanned waterborne surface vehicle that 
are present; generating a command to be issued to the 
umnanned waterborne surface veliicle to operate at a particu- 
lar velocity and along a particular heading, so as to approach 
the desired location while avoiding a collision with each of 
35 the one or more objects different from the unmanned water- 
borne surface vehicle that are present; and iteratively repeat- 
ing the above steps until the desired location is reached. 

In still a further embodiment, the desired location is a 
location proximate to one of the obj ects deduced to be present 
40 in step (iii). 

In one embodiment, the desired location is a predicted 
location of one of the objects deduced to be present in step 
(iii). 

In another embodiment, the second set of rules includes 
45 COLREGS rules. 

In yet another embodiment, the one or more sensors are 
selected from the group of sensors consisting of radar, sonar, 
lidar and stereo cameras. 

In still another embodiment, the object different from the 
50 unmanned waterborne surface vehicle is an object of interest 
to be followed. 

The foregoing and other objects, aspects, features, and 
advantages of the invention will become more apparent from 
the following description and from the claims. 

55 

BRIEF DESCRIPTION OF THE DRAWINGS 

The objects and features of the invention can be better 
understood with reference to the drawings described below, 
60 and the claims. The drawings are not necessarily to scale, 
emphasis instead generally being placed upon illustrating the 
principles of the invention. In the drawings, like numerals are 
used to indicate like parts throughout the various views. 

FIG. lA through FIG. ID illustrate maneuvers required for 
65 various COLREGS situations. 

FIG. 2A is a diagram of a situation in which COLREGS 
might not apply. 
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FIG. 2B is a diagram of a situation in which COLREGS 
might apply even with the identical geometric setup as in FIG. 

2A. 

FIG. 2C is a flowchart showing how the software structure 
of the present invention is decomposed into a strategic plan- 5 
ner and a local planner. 

FIG. 3A is a diagram illustrating the positions and relative 
velocities of a USV and a potential hazard. 

FIG. 3B is a diagram that provides a graphical interpreta- 
tion of a Velocity Obstacle (VO). When the relative velocity to 
of the robot v^-v^ points inside the cone formed by the robot 
center and the expanded obstacle A0B, they will collide. The 
Velocity Obstacle imposed on robot’s velocity is this cone 
shifted by the vector. 

FIG. 4 is a diagram illustrating a Worst case Velocity 15 
Obstacle (WVO) when there are uncertainties in the velocity 
of the moving obstacle. 

FIG. 5 is a diagram illustrating the constraints in the veloc- 
ity space imposed by COLREGS. 

FIG. 6 is a diagram illustrating the Closest Point of 20 
Approach (CPA). 

FIG. 7 is an example of a Target Detectibility (TD) Map for 
a given heading and time. 

FIG. 8A is a diagram that illustrates the first waypoint goal 
(t=ti) in which the USV is behind the centroid of the prob- 25 
ability map (d>0). 

FIG. 8B is a diagram that illustrates the first waypoint goal 
(t=ti) in which the centroid of the probability map is behind 
the USV (d<0). 

FIG. 9A is an image of a CMV, the USV boat used in the 30 
on-water COLREGS testing. 

FIG. 9B is an image of a 1 2 m traffic boat. 

FIG. 9C is an image of a 7 m RHIB traffic boat. 

FIG. 10 is an image of the JPL stereo cameras (outlined in 
the ellipse) on the boat. 35 

FIG. 11 is a diagram illustrating a simulation run in which 
the USV is in a crossing situation. 

FIG. 12A1 is a composite image, in which the lower panel 
is a plot of data and the upper panel is a pair of stereo optical 
images. In FIG. 12A1 the USV detects one of the 7 m RHIBs, 40 
but the estimated velocity is low enough and USV simply 
treats it as a stationary hazard. No COLREGS constraint is 
applied. 

FIG. 12A2 is a path diagram corresponding to FIG. 12A1. 

FIG. 12B1 is a composite image, in which the lower panel 45 
is a plot of data and the upper panel is a pair of stereo optical 
images. In FIG. 12B1 the 7 m RHIB approaches, the USV 
recognizes the head-on situation, and applies the COLREGS 
constraint. 

FIG. 12B2 is a path diagram corresponding to FIG. 12B1. 50 

FIG. 12C1 is a composite image, in which the lower panel 
is a plot of data and the upper panel is a pair of stereo optical 
images. In FIG. 12C1 the USV detects two head-on vessels, 
and avoids them by moving further to the starboard. The 1 2 
meter boat is visible in the image, but is not detected by the 55 
tracker. 

FIG. 12C2 is a path diagram corresponding to FIG. 12C1. 

FIG. 12D1 is a composite image, in which the lower panel 
is a plot of data and the upper panel is a pair of stereo optical 
images. In FIG. 12D1 the USV avoids all three vehicles. The 60 
USV treat the 12 meter vessel to be in both the crossing 
situation and the head-on situation. 

FIG. 12D2 is a path diagram corresponding to FIG. 12D1. 

FIG. 12E1 is a composite image, in which the lower panel 
is a plot of data and the upper panel is a pair of stereo optical 65 
images. In FIG. 12E1 the USV successfully avoids the 12 
meter boat. 
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FIG. 12E2 is a path diagram corresponding to FIG. 12E1. 

FIG. 12F is a diagram illustrating the overall path. 

FIG. 13A1 is a composite image, in which the lower panel 
is a plot of data and the upper panel is a pair of stereo optical 
images. In FIG. 13A1 the USV recognizes the overtaking 
situation, and initiate a turn maneuver going starboards to 
obey COLREGS, even though the straight line to the way- 
point passes on the port side of the traffic boat. 

FIG. 13 A2 is a path diagram corresponding to FIG. 13 Al. 

FIG. 13B1 is a composite image, in which the lower panel 
is a plot of data and the upper panel is a pair of stereo optical 
images. In FIG. 13B1, during the overtaking, the USV detects 
another vehicle in the head-on situation. 

FIG. 13B2 is a path diagram corresponding to FIG. 13B1. 

FIG. 13C1 is a composite image, in which the lower panel 
is a plot of data and the upper panel is a pair of stereo optical 
images. In FIG. 13C1 the USV successfully avoids the head- 
on vehicle. 

FIG. 13C2 is a path diagram corresponding to FIG. 13C1. 

FIG. 13D1 is a composite image, in which the lower panel 
is a plot of data and the upper panel is a pair of stereo optical 
images. In FIG. 13D1 the USV is now in both the head-on and 
crossing situation. It veers to its starboard to let the 12 meter 
boat maintain its course, because the 12 meter boat is the 
stand-on vessel in this situation. 

FIG. 13D2 is a path diagram corresponding to FIG. 13D1. 

FIG. 13E1 is a composite image, in which the lower panel 
is a plot of data and the upper panel is a pair of stereo optical 
images. In FIG. 13E1 the USV successfully avoids the cross- 
ing vehicle. 

FIG. 13E2 is a path diagram corresponding to FIG. 13E1. 

FIG. 13F1 is a composite image, in which the lower panel 
is a plot of data and the upper panel is a pair of stereo optical 
images. In FIG. 13F1, after the successful avoidance of the 12 
meter vessel, the boat being overtaken comes in the view 
again. 

FIG. 13F2 is a path diagram corresponding to FIG. 13F1. 

FIG. 13G is a diagram illustrating the overall path. 

FIG. 14A is an image of the USV, a PowerVent boat used in 
the on- water testing. 

FIG. 14B is an image of an 1 1 m RHIB, the traffic boat. 

FIG. 15A is a diagram that illustrates the submarine 
maneuvering to the right as the USV trails. 

FIG. 15B is a diagram that illustrates that the submarine 
has slowed down, and has turned 180 degrees and is now 
headed at the USV. 

FIG. 15C is a diagram that illustrates that the USV has 
maintained trail tlirough a 360 degree turn. 

FIG. 16 is a diagram that illustrates how the USV navigates 
around a buoy while trailing the submarine. 

FIG. 17 is a diagram that illustrates a COLREGS crossing 
situation during trailing. 

FIG. 18 illustrates a COLREGS head-on situation during 
trailing. 

DETAILED DESCRIPTION 

This technology uses hardware to obtain data about the 
objects in the vicinity of a maritime autonomous surface 
vehicle (ASV), also referred to as an autonomous waterborne 
vessel, and uses software to implement a motion planning 
module to accomplish a desired task, such as trailing an obj ect 
of interest in a safe manner. The software module is designed 
to trail a designated “target” while also avoiding static and 
dynamic surface hazards. When the system detects surface 
hazards such as other moving boats, the motion planner 
applies International Regulations for Preventing Collisions at 
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Sea (COLREGS); an important subset of these rules has been 
implemented in the software. In addition, in the case that 
contact with the target is lost, the software can receive and 
follow a “reacquisition route”, provided by a complementary 
system, until the target is reacquired. The invention can pro- 5 
vide the ASV with the following types of capabilities: the 
ASV can “shadow” or “keep tabs on” another waterborne 
vehicle; the invention can provide the ASV the capability to 
catch another waterborne vehicle that is out of control; the 
invention can provide the ASV the capability to interdict or 
intercept another waterborne vehicle; and the invention can 
provide the ASV the capability to autonomously and safely 
negotiate a path from a first known location (e.g., point 1 on 
a map) to another known location (e.g., point 2 on a map) for 
example to provide an autonomous ferry service between the 
two points. 

Problem Statement 

The software receives as input the following data: 

A list of surrounding sensor contacts, which might be buoys, 20 
other boats, etc., with position, velocity, and uncertainty 
information, which are primarily objects to be avoided; and 
the “target” is designated in this list; 

For the case of active contact with the target: a set of predicted 
“detectability maps”, indicating desirable ASV poses at par- 25 
ticular future time points; 

For the case of lost contact with the target: a “reacquisition 
route”, providing desirable waypoints for the ASV to regain 
contact with the target; 

Ownship state information from an inertial navigation system 
(INS); and nautical chart data indicating safe water regions 
(optional, for littoral regions). 

Given the above information, the motion planning software 
attempts to maintain contact with the target as much as pos- 
sible, while making certain to avoid collisions with static and 
dynamic hazards. When a COLREGS scenario is encoun- 
tered, the motion planner needs to recognize the scenario and 
follow the evasive course designated by these “rules of the 
road.” A subset of the COLREGS rules has been imple- 40 
mented, which includes head-to-head, crossing, and overtake 
situations. If contact with the target is lost and the motion 
planner receives a reacquisition route, the software will fol- 
low the reacquisition route while also applying its safe navi- 
gation rules. Note that, because trailing and reacquisition are 45 
time-sensitive, the ASV goal locations have “arrival times” at 
which the ASV should be at the location. Additionally, 
because the target detection sensor may be orientation spe- 
cific, the goal locations may include particular heading values 
to point the sensor appropriately. It is assumed that the ASV 50 
has at least a slight advantage over the target in terms of speed 
and maneuverability. 

Integrating track-and-trail behavior with safe navigation 
(hazard avoidance and COLREGS) is the crux of this motion 
planning software. There are several challenges to be faced 55 
when merging and arbitrating between these behaviors, 
including: 

Dealing with Multiple Objectives 

A difficult aspect of the problem of combining competing 
behaviors is deciding how to choose the ASV motion when 60 
objectives are competing to move differently (e.g., trail vs. 
COLREGS). This can be considered an act of “arbitration” 
(selecting the one behavior to “pay attention to”) or “multi- 
objective optimization” (combining the behaviors in a way 
that best compromises between all objectives). Further, some 65 
objectives might be mandatory (e.g., avoid collisions), while 
others are desirable but not as critical (e.g., COLREGS). 


Temporal Considerations 

Most previous hazard avoidance motion planning 
approaches just try to reach a “waypoint” safely. When trail- 
ing, however, the time it takes to maneuver is of consequence, 
as the ASV must reposition itself correctly to keep acquisition 
of the target. 

Unstructured, Dynamic Environment 

Other agents in the environment are moving, and, unlike 
some environments such as roadways, there is little to no 
strucmre to rely on for guiding the motion of agents or the 
robot ASV. 

Uncertainty 

The software must account for uncertainty of target and 
traffic, including sensor uncertainty of the current states and 
prediction uncertainty of future actions. 

Noise 

The environment is very noisy, both for measurement and 
motion control. Significant motion due to vehicle roll/pitch/ 
yaw exists due to sea motion, which is an issue unique to 
maritime vehicles. 

Power and Processor Limitations 

The motion planning software runs on a real-time embed- 
ded system, where power and processing speed are limited. 
Thus, the choice of algorithms must account for these real- 
world limitations in computational capability. 

Strucmre 

The algoritlmiic approach taken in one embodiment is to 
decompose the problem into a global/strategic planner and a 
local planner (See FIG. 2C). FIG. 2C is a flowchart showing 
how the software strucmre of the present invention is decom- 
posed into a strategic planner (the outer loop 220) and a local 
planner (the inner loop 230). The outer loop inputs 210 
include a contact list; INS data; DNC/ENC; a target 
detectibility map; and a reacquisition trajectory. The role of 
the strategic planner is to interpret the target detectability 
maps and reacquisition routes and convert them into mid- 
level waypoint goal queue for the ASV. These waypoint goals 
may have arrival times and desired headings specified as well 
as two-dimensional surface position. The role of the local 
planner is to achieve these waypoint goals safely (or deter- 
mine that they are not reachable), that is, avoiding hazards and 
applying COLREGS rules. The output 240 of the software 
module is a velocity command for the ASV, including a veloc- 
ity magnitude and a heading. 

The outer loop provides a strategic planner that sets way- 
point goals based on mission mode and mission objectives. 
The outer loop deals with reaching a specific point, whether 
defined as a fixed point on a map, or defined as a predicted 
location of another waterborne vessel (“the other vessel”) at a 
later time. The other vessel can be a surface vessel or a vessel 
capable of operating beneath the surface of the water, such as 
a submarine. The outer loop performs the steps of: interpret- 
ing the target detectibility map; translating the reacquisition 
trajectory; accepting manual commands; setting strategic 
goals based on expected arrival time, geography, and other 
parameters. 

The inner loop provides a local planner that ensures safe 
navigation. The inner loop uses the information about the 
location that the vessel is trying to reach and computes a 
possible path that will be a safe path. The inner loop performs 
the steps of: avoiding static and dynamic obstacles; applying 
the COLREGS rules; balancing the motion needed to accom- 
plish the goal and operating under safe navigation conditions ; 
and updating the sensor data. The inner loop provides as 
output a velocity command and a heading command (or a 
time series of such commands) that are acted upon by the 
autonomous waterborne vessel. 
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Provision for a human operator to issue commands that can 
override the computed commands is provided as well. The 
human operator can be located on the autonomous water- 
borne vessel, or the human operator can be located away from 
the autonomous waterborne vessel, and can observe events 
remotely by telemetry (e.g., visual images, data, and/or other 
signals that contain information about the autonomous water- 
borne vessel and its surroundings) and can issue commands 
from the remote location. 

We now describe an autonomous motion planning algo- 
rithm for Unmanned Surface Vehicles (USVs) or maritime 
autonomous surface vehicle (AS Vs) to navigate safely in 
dynamic, cluttered environments. The algoritlnn not only 
addresses Hazard Avoidance (HA) for stationary and moving 
hazards, but also applies the International Regulations for 
Preventing Collisions at Sea (COLREGS). The COLREGS 
rules specify, for example, which vessel is responsible for 
giving way to the other and to which side of the “stand-on” 
vessel to maneuver. Three primary COLREGS rules are con- 
sidered: crossing, overtaking, and head-on simations. For 
autonomous USVs (or ASVs) to be safely deployed in envi- 
ronments with other traffic boats, it is imperative that the 
USV’s navigation algorithm obeys COLREGS. Furthermore, 
when other boats disregard their responsibility under COL- 
REGS, the USV must fall back to its HA algorithms to pre- 
vent a collision. The approach used in one embodiment is 
based on the Velocity Obstacles (VO) method, which gener- 
ates a cone-shaped obstacle in the velocity space. Because 
Velocity Obstacles also specify on which side of the obstacle 
the vehicle will pass during the avoidance maneuver, COL- 
REGS are encoded in the velocity space in a natural way. This 
motion planner is used for the task of following a target, 
where the Strategic Planner commands USV waypoints 
based on high-level objectives and the local planner ensures 
hazard avoidance and compliance with COLREGS during 
traverse. We present details of a specific strategic planner 
which calculates waypoint goals for trailing a target vessel. 
Results from several experiments involving up to four vessels 
are presented, in what we believe is the first on-water dem- 
onstration of autonomous COLREGS maneuvers without 
explicit inter-vehicle communication. 

There are actually separate sets of niles for international 
waters and, for example, inland waters in the United States. 
We consider the international regulations specified by the 
1972 COLREGS. We assume all surface traffic consists of 
roughly equivalent power-driven vessels. Different COL- 
REGS priorities apply to sailing vessels and other special 
situations. Consequently, as autonomous USVs are operated 
alongside other manned and unmarmed vehicles, they need to 
be able to safely avoid other vessels. The present systems and 
methods can be augmented to include other COLREGS rules, 
which have not yet been implemented in the embodiments 
described hereinbelow. 

The Velocity Obstacle (VO) approach has been adopted by 
several researchers formoving hazard avoidance. Since it was 
first proposed in 1998 for robot motion planning (see P. 
Fiorini and Z. Shiller, “Motion Planning in Dynamic Envi- 
ronments Using Velocity Obstacles,” International Journal of 
Robotics Research, vol. 17, no. 7, pp. 760-772, 1998), several 
extensions to VO have been made, including a cooperative 
form of collision avoidance (see J. van den Berg, M. Lin, and 
D. Manocha, “Reciprocal Velocity Obstacles for Real-Time 
Multi-Agent Navigation,” in Proceedings of the IEEE Inter- 
national Conference on Robotics and Automation, 2008, pp. 
1928-1935), probabilistic velocity obstacles (see B. Kluge 
and E. Prassler, “Reflective Navigation: Individual Behaviors 
and Group Behaviors,” in Proceedings of the IEEE Intema- 
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tional Conference on Robotics and Automation, 2004; and C. 
Fulgenzi, A. Spalanzani, and C. Laugier, “Dynamic Obstacle 
Avoidance in uncertain environment combining PVOs and 
Occupancy Grid,” in Proceedings of the IEEE International 
5 Conference on Robotics and Automation, 2007), and crowd 
simulation (see J. van den Beig, S. Patil, J. Sewall, D. 
Manocha, and M. C. Lin, “Interactive Navigation of Multiple 
Agents in Crowded Environments,” in Symposium on Inter- 
active 3D Graphics and Games, 2008). VO approaches gen- 
10 erate a cone-shaped obstacle in the velocity space (hence the 
name Velocity Obstacles) and ensure that there will be no 
future collisions as long as the robot’s velocity vector is 
outside of the VO. In order to identily the risk of future 
collisions, one could predict both the pose of the moving 
15 hazard and the pose of the robot for several time steps into the 
future, and perform collision checks using their configura- 
tions at each time slice. This approach has the advantage that 
it can check collisions of vehicles following arbitrary trajec- 
tories. However, because it needs to perform collision checks 
20 at many time slices, the computational load becomes very 
high. On the other hand, VO makes a first-order (i.e., linear) 
prediction, and the collision check is done in the velocity 
space. Since a single collision check accounts for collision 
checks at all future times (due to the linear velocity assump- 
25 tion), VO is very fast to compute and is well-suited for robotic 
applications where the algoritlim is implemented on embed- 
ded systems that have limited computational resources and 
hard real-time constraints. 

We extend VO in the context of maritime navigation sub- 
30 ject to COLREGS. In particular, we use VO to avoid moving 
and static hazards, but also generate an additional set of 
constraints in the velocity space when the USV is in certain 
COLREGS situations. Because both VO and COLREGS are 
defined in vehicle’s body-fixed frame, COLREGS constraints 
35 are expressed together with VO in a very natural way. 

In addition, we discuss how the above “safe navigation” 
approach fits into the context of an algorithm for autonomous 
maritime target trailing. Typically, VO approaches assume a 
goal location (more precisely, a goal velocity) has been iden- 
40 tilled in advance. Thus, we embed our VO approach as a local 
planner for reaching goals safely, while a more general stra- 
tegic planner sets the waypoint goals based on the USV’s 
mission objectives. Here, we present a strategic planner that 
aims to track (i.e., maintain sensor contact with) a target 
45 vessel and trail the target over time. Our trailing problem is 
difficult because (a) the target vessel is allowed different 
capabilities from the USV (e.g., a submarine that can travel 
underneath the surface traffic) and (b) we assume the target 
sensor is directional, placing strong restrictions on the USV’ s 
50 heading as well as its surface position. We address this by 
using predictive “target detectability maps” (TD Maps) to 
calculate 4-dimensional waypoint goals (x, y, heading, and 
arrival time) for the local plarmer to reach. 

Review of COLREGS 

55 We address the following three primary COLREGS situa- 
tions: crossing, head-on, and overtaking. FIG. lA through 
FIG. ID illustrate the rules in these situations. The darker 
triangle represents the robot (USV), and the lighter triangle 
represents the traffic boat. In FIG. lA, the traffic boat is 
60 crossing from the right. The COLREGS rule states that in this 
situation, the vessel that has the other on its starboard (right) 
side must give way. Therefore, the USV (the “give-way” 
vessel) must avoid the traffic boat (the “stand-on” vessel), and 
the traffic boat does not need to alter its path. In FIG. IB, the 
65 traffic boat is crossing from the left. In this case, the traffic 
boat is the give-way vessel, and USV should be allowed to 
maintain its course. We assume all surface traffic consists of 
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roughly equivalent power-driven vessels. Different COL- 
REGS priorities apply to sailing vessels and other special 
situations. In FIG. 1C, the USV is overtaking a slow traffic 
boat. In this situation, the USV must ensure enough clear- 
ance, so that it keeps out of the way of the traffic boat being 
overtaken. Although COLREGS do not specify which side of 
the boat it must overtake, common practice on the water 
dictates that the overtaking boat should pass on the right side 
of the traffic boat. In FIG. ID, the USV and the traffic boat are 
moving straight toward each other, head-on. Flere, both ves- 
sels must alter their course toward the starboard, so that they 
pass with the other vessel to its port (left) side. 

Even in a simple scenario, whether or not a COLREGS rule 
applies is not a trivial evaluation to make, especially as the 
rules are written for human operators and often include sub- 
jective measures. Nor is the evaluation a simple function of 
the vehicles’ relative position (e.g., bearing angle or distance) 
and/or vehicle heading. For example, as shown in FIG. 2A 
and FIG. 2B, even when the traffic vessel and USV are in the 
same geometric configuration, the crossing rule might or 
might not apply, depending on their relative speeds and the 
separation between the vessels. 

COLREGS Motion Planner 
Problem Statement 

The problem considered for the Local Planner is stated as 
follows. Given a near-term waypoint, a reference speed, and 
a list of contacts representing moving and static hazards, find 
the best velocity command that avoids the hazards and obeys 
COLREGS. 

Review of Velocity Obstacles 

We briefly review the Velocity Obstacles approach to robot 
motion planning that serves as a foundation for this work. Let 
us first introduce some mathematical notations. Let pEK ^ 
denote the robot’s position vector and vEK ^ denote the 
velocity vector in a two dimensional space. A ray starting 
from p going into the direction of v is defined as 

X(p,v)=fy+A'lffiO}. (1) 

Furthermore, the following set operations are used to 
express the VO. 

Minkowski sum: 0 S ={u+tJ|aEv4 ^ } (2) 

Reflection:- v4={-u|uE A } f3) 

Then, given a robot of shape A and a obstacle of shape B 
moving at velocity v^, the VO of obstacle B in the velocity 
space of robot A is given as 

vo^-^(v^)={v^ ik(p^,v^-v^)n( Be- A ),0} (4) 

where p^ and p^ are the position of the robot and the obstacle, 
andv^ and are their velocity vectors, respectively. A simple 

interpretation of (4) is that the ray starting from robot A and 
going in the direction of the relative velocity (v^-v^) inter- 
sects the obstacle B expanded by the robot size A. The reason 
for this C-space expansion is the ability to treat the robot as a 
point. FIG. 3A is a diagram illustrating the positions and 
relative velocities of a rectangular robot USV and a rectan- 
gular moving hazard. FIG. 3B is a diagram that provides a 
graphical interpretation of a Velocity Obstacle (VO). When 
the relative velocity of the robot v^-v^ points inside the cone 
formed by the robot center and the expanded obstacle A0B, 
they will collide. The Velocity Obstacle imposed on robot’s 
velocity is this cone shifted by the vector. As shown in 
this example, a VO is a cone in the velocity space. 

As long as the robot’ s velocity lies outside of VO, it will not 
collide with the obstacle, assuming that the velocity vectors 
are constant over time. If the velocity vectors change over 
time, the VO-based approach reacts by re-planning using the 
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latest sensor information. In practice, when the re-planning 
rate is much faster than the change in the vehicles’ trajecto- 
ries, the assumption of linear velocities is reasonable. In one 
embodiment, the re-plan rate is 1 Hz on an embedded system, 
5 and the angular velocity of the boat is typically less than 30 
degrees per second. This re-plan rate can be increased, as the 
motion planning computation itself is very fast (on the order 
of several milliseconds). 

When multiple craft are involved, simply overlaying the 
to VO of each ship and taking a superposition generates a set of 
constraints on the robot’s feasible velocity vector. The run 
time of the VO-based approach is at worst linear with the 
number of hazards considered. 

Time-to-Collision 

15 When the velocity is inside the velocity obstacle, the 
time-to-collision x can be obtained by computing the time it 
takes for the relative velocity vector v^-v^ to intersect the 
boundary of ,4. 0 S , i.e., 

2Q PAMxj-VB)ed( B^-A) (5) 

where 3(*) denotes the boundary of the set. If there are mul- 
tiple x’s that satisfy (5), the one with the minimum value is 
selected. 

Uncertainty Handling 

25 The motion platmer that is deployed in the real world must 
account for various types of uncertainties. Moving hazards 
are detected and tracked using on-board sensors such as radar 
(see L. Elkins, D. Sellers, and W. R. Monach, “The autono- 
mous maritime navigation (AMN) project: Field tests, 
30 autonomous and cooperative behaviors, data fusion, sensors, 
and vehicles,” Journal of Field Robotics, vol. 27, no. 6, pp. 
790-818, 201 0), cameras (see T. Huntsberger, H. Aghazarian, 
A. Howard, and D. C. Trotz, “Stereo vision-based navigation 
for autonomous surface vessels,” Journal of Field Robotics, 
35 vol. 28, no. 1, pp. 3-18, 201 1), and lidar. Performance char- 
acteristics of the sensor in turn affect the noise and state 
estimation errors for the tracked traffic boat. 

Another source of uncertainty comes from the imprecise 
motion of the moving hazards. VO assumes constant veloci- 
4 Q ties of the moving agents, but in reality their trajectory does 
not necessarily maintain a constant velocity. In order to 
account for such uncertainties in the traffic vehicle, its veloc- 
ity Vg is modeled as Vg=Vg+bg, where is the nominal 
velocity (i.e., the expected velocity that is estimated by the 
vehicle tracker), and bg captures the uncertainties of the vehi- 
cle’s velocity. We assume that the uncertain component of the 
velocity lies in a set S^E W g, where W ^ is a bounded set and 
is treated as a constant. Then, the velocity obstacle with the 
worst-case uncertainty (denoted by “WVO”) is written as 

wvo/K)=vo/N«)0W^. (6) 

FIG. 4 is a diagram illustrating a Worst case Velocity 
Obstacle (WVO) when there are uncertainties in the velocity 
of the moving obstacle. FIG. 4 shows the same case that was 
shown in FIG. 3B but with uncertainties. The gray cone shows 
the VO with the nominal velocity. In this example, the moving 
hazard B has a velocity uncertainty W g, which is drawn with 
an ellipse centered at the nominal velocity Vg. The worst case 
VO is slightly larger than the nominal VO, and the boundary 
lines are parallel to those of the VO. As shown later, the VO is 
treated as a hard constraint, but the region W ^ of WVO is 
60 treated as a soft constraint to provide a safety buffer. 

VO with COLREGS 

VO is a set of constraints in the USV’s velocity space 
needed to avoid moving obstacles. Our approach treats COL- 
REGS as an additional set of constraints in the velocity space. 
65 One advantage of using VO to encode COLREGS is that the 
VO already has the information on which side of the hazard 
the USV should pass. FIG. 5 is a diagram illustrating the 
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constraints in the velocity space imposed by COLREGS. 
FIG. 5 shows a single VO and a feasible velocity space 
divided into three regions. The line dividing V3 from Vj and 
V2 is tangent to WVO^'^(v^) and is perpendicular to the cen- 
terline of the VO cone, which is parallel to p^-p^, the line 5 
connecting the center of the US V and the center of the moving 
obstacle. When USV follows a velocity in V3, the relative 
velocity of USV points away from the obstacle, i.e., 

SO that the vehicles do not approach any closer. 

When USV follows a velocity in Vj, 

V j={v iv^wvo2j'<(v2,),v^ V 

Was.t.[fe-;).<)x(v.<-v2j)F<0} 
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then the USV will pass the obstacle while seeing it on the right 
side. Flere, the operator extracts the z component of the 
vector. We use the conventional body-fixed frame, with +x 
pointing forward, +y pointing right, and +z pointing down- 
ward. This region Vj is treated as inadmissible when the USV 20 
is overtaking the traffic vessel B, when they are in a head-on 
situation, and when the traffic vessel B is crossing from the 
right. Note that when v^EV^ , USV will see the vessel B on the 
starboard side, thus cutting in front of the vessel B. 

This rule-based constraint v^^Vj ensures that the USV 25 
will only pass from the right (when v^EVj) or does not pass 
(when v^EVj). 

Note that when vessel B is crossing from the left, there is no 
COLREGS constraint on USV because vessel B is respon- 
sible for avoiding the USV. Even without the COLREGS 39 
constraint, however, VO always exists and hence the USV 
will avoid any moving hazard. This is necessary to ensure 
safety in case vessel B violates COLREGS and does not take 
any avoidance measures. Similarly, while being overtaken, no 
COLREGS constraints are applied. 35 

Algorithm Flow 

The first step in the algorithm flow is done by a nile selec- 
tor, whose role is to detect if a moving vessel is in a specific 
COLREGS situation with respect to the US V. As illustrated in 
FIG. 2 A and FIG. 2 B, it is not sufficient to analyze only the 49 
geometric relation between the USV and moving hazards. 

The procedure used in one embodiment includes the fol- 
lowing: 

Pre-Collision Check 

The rule selector first computes a Closest Point of 45 
Approach (CPA) with the current position and velocity of the 
USV and traffic vessels, and evaluates if any COLREGS niles 
need to be applied at all. This approach ensures that we apply 
COLREGS only to the relevant vehicles and is more efficient 
than computing CPA for every potential USV velocity with 59 
every moving vessel. 

The CPA between two vessels is computed as follows. 
Given two points p^, p^ and their velocity vectors v^, v^, the 
time to CPA ^cpa is given by 


^CPA 


[ ^ 

\ ip A -V5) 

i I|V 3 -Vb|P 


if I|vb -vbII <£ 
otherwise. 


( 7 ) 
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as shown inFIG.6.FIG.6isa diagram illustrating the Closest 
Point of Approach. Each dot corresponds to a position at a 
specific time, and the dashed lines show the distance between 
two agents A and B. The closest distance is marked with a 
thick bar. The distance at CPA is then computed by 


‘^CPA I \(Pa+''^a^cba)~(Pb+'^b^cpa)\1 


( 8 ) 


For each moving vessel, the motion planner examines 
whether COLREGS rules apply only if the situation is likely 
to lead to a collision or a near-collision in the short-term 
fiiture, that is, by checking if 

dcpA^d^i„- ( 9 ) 

Rule Selection: 

Once the CPA meets the condition ( 9 ), the rule selector 
identifies which COLREGS situation applies. Because 
“crossing from the left” and “crossing from the right” require 
USV to perform distinctly different maneuvers, the rule 
selector considers the following four situations and their 
respective rules: overtake, head-on, crossing from the left, 
and crossing from the right. 

The rule selector decides that it is in an overtake situation 
when the following conditions are all satisfied. 

Distance:\\pj~p^\\^D^^ 

Heading: 

Cross-track:rns:r„,,i„ 

Aiong-track:a:5£a:„„-„ 

where and 6^ are the heading direction of the current 
velocity vector, and (x^, y^) are the vehicle B’s position 
measured in A’s body-fixed frame. 

Similarly, it is considered to be in the head-on situation 
when the following conditions are all satisfied. 

Distance:|^^-/jBl|sZ)„,„,. 

Heading:l0^-0B+jils0„,,j,. 

Cross-trackirn^j',,,,,,. 

Along-track:a:Baa:„,,„ 

It is considered to be in the “crossing from the right” when 
the following conditions are all satisfied. 

Heading: 0 „,,„s 0 B- 0 ^se„„,„. 


Bearing:6„„.„s6 B^^max 
Heading-Bearing relation: 05 >t)B-jt 
Cross-track: 3 ^BS 3 ^„,,„ 

where b^ denotes the relative bearing of B from the USV. The 
third bullet is to ensure that the moving hazard is not coming 
up behind the USV. 

It is considered to be in the “crossing from the left” when 
the following conditions are all satisfied. 

Heading: 0 „„-„^ 0 B- 03 + 3 t^ 0 „,„„ 


Cross-track:3^g^-_r„„-„ 

Hysteresis 

If a completely new decision is made at every time step, as 
in the CPA-based rule selection discussed above, the USV 
might end up with a “chattering” behavior. This is because the 
uncertainty in the situational awareness could make the COL- 
REGS constraints turn on and turn off very frequently, which 
drastically alters the feasible region of the decision space, as 
shown in Vj of FIG. 5 . FIG. 5 is a diagram illustrating the 
constraints in the velocity space imposed by COLREGS. 

In order to alleviate churning, in one embodiment we intro- 
duce hysteresis to the rule selector and lower the bandwidth of 
the system. It also has the added benefit that once a COL- 
REGS maneuver (e.g., overtaking, crossing) is initiated, it 
continues to direct the boat for at least a minimum duration of 
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time, thereby making the USV’s decision more obvious and 
predictable to human drivers on other vessels. 

Because the moving obstacles are output by the vehicle 
tracker, each moving obstacle is assigned a unique ID. Using 
this vehicle ID, we maintain a short history of vehicle-rule 5 
pairs, i.e., which vehicle meets the criteria of which rule. If a 
vehicle meets the criteria of a certain COLREGS rule, it is 
considered to be in a COLREGS situation. However, even if 
a vehicle does not meet the criteria at the current time step, as 
long as the vehicle met the criteria in any of the last % time 
steps, we still apply the corresponding COLREGS rule. Infor- 
mally, the parameter % is a length of the hysteresis and 
controls how often the USV can “change its mind” in the 
COLREGS situations. If the vehicle does not meet the criteria 
% times in a row, then the corresponding vehicle-rule pair is 
removed from memory. 

In some embodiments multiple COLREGS rules could be 
active at the same time with the same traffic vehicle. For 
example, if the boat is coming from a diagonal direction 20 
around the boundary of crossing and head-on direction, one 
might want to apply both rules in order to mitigate sensing 
noise and to ensure vehicle safety. Constraints for multiple 
COLREGS rules in the velocity space can be easily generated 
by simply superposing each COLREGS-based constraint to 25 
form a net constraint for that vehicle. 

Cost 

We use a regular discrete grid in the v-9 space to find the 
best velocity vector, where v is the speed of the USV. Once the 
constraint sets of VO and COLREGS are generated, for each 30 
Vj and 0^. that is still admissible, the vehicle computes the 
following cost: 


Jij = 


— + Wv 


\^ref - 


ViCosOj 1 
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where w.„ is a weight on the time-to-collision, a desired 

velocity to reach the next near-term goal, w„ is a weight on the 40 
deviation from desired velocity, and Il'Hg is a weighted two- 
norm of a vector. The weighting matrix Q of size 2-by-2 
allows us to weigh the deviation of the along-track velocity 
and the cross-track velocity differently. If (Vj,0y)GWVO and 
(v„0^)^VO, then the weight w.^ on time-to-collision is 45 
reduced in order to soften the constraints on WVO. Once all 
the costs are computed, the (v,., 0^.) pair with the minimum cost 
is selected and the velocity command is sent to the vehicle 
controller. 

In principle, even though VO “looks ahead” into the future 50 
of the robot and moving agents, it is possible to construct a 
case where the robot is trapped because VO is a local planner. 
However, such a situation is unlikely to happen in practice, 
especially for on-water applications, where there are typically 
only a few vehicles that need to be avoided simultaneously. If 55 
one needs to generate a path in a maze-like environment, 
while avoiding moving hazards, it can be combined with a 
global path planner that takes into account the obstacles at a 
longer range. The next section explains how the COLREGS 
planner presented in this section is used as a local planner for 60 
more complex USV missions. 

Application to Target Trailing 

The problem statement considered in one embodiment is to 
trail a moving submarine using a forward-looking sonar sen- 
sor, while avoiding hazards and obeying COLREGS with 65 
surface traffic vessels. The sonar has a limited field of view 
and a limited detection range, but it is important for the USV 


to stay behind the submarine at some distance in order to track 
the submarine. The difference between the USV and subma- 
rine capabilities also makes the problem challenging, because 
while the USV must obey COLREGS to avoid traffic vessels 
on the water, the submarine in many instances can simply 
pass underneath them, if the water is deep enough. 

In one embodiment, we decompose the problem into three 
components: generation of a target detectability map, strate- 
gic planning, and local planning. The target detectability map 
(TD Map) predicts the probability of tracking the submarine 
from a range of surface poses. The strategic planner uses the 
TD Map to generate waypoint goals that are most likely to 
maintain contact with the target. The local planner, for which 
we employ the approach presented hereinabove, is then 
responsible for reaching the waypoint goals while avoiding 
hazards and obeying COLREGS. In a dynamic and uncertain 
enviroimient, the TD Map is frequently updated, and the 
strategic planner makes adjustments to the near-term goal for 
the local planner. This approach is shown to be sufficient to 
maintain the trail of the target under a wide variety of taiget 
maneuvers and surface conditions, as will be discussed here- 
inbelow. 

Target Detectability Map 

The TD Maps include a set of predicted target detection 
probabilities for a range of possible USV poses at selected 
times in the future. They are generated via a model-based 
agent-simulation (see W. Monach, R McMorran, and T. 
McSherry, “Non-Gaussian Tracking Engine (NOTE) Algo- 
rithm Description,” Daniel H. Wagner Associates,” Report to 
NSWC-DD, August 2008), which accounts for the limited 
field of view of the sensor, uncertainties in the motion model 
of the target, and the interaction of the target with the envi- 
ronment. The TD Map is represented as a four dimensional 
grid (2D surface position, heading, and time), and each cell in 
the grid has a probability P(x, 0, t) of detecting the target if the 
USV is at the position x with the heading 0 at time t. An 
example of a TD Map for a particular heading and a particular 
time is shown in FIG. 7. In FIG. 7 lighter colors indicate a 
higher probability of target detection at those locations. The 
primary purpose of the TD Map is to encode the sensor model 
in a grid, so that a computationally demanding sensor model 
calculation can be carried out outside of the planning mod- 
ules. 

Strategic Planner 

The Strategic Plaimer receives TD Maps and outputs a 
waypoint goal to maintain target tracking. In addition to indi- 
cating two-dimensional surface positions, the waypoint goal 
has a desired heading and an arrival time, for a total of four 
dimensions. The desired heading is required to ensure direc- 
tional sensors are pointed toward the target, and the arrival 
time information allows the algorithm to synchronize the 
USV’s motion with that of the moving taiget. 

A sequence of waypoints p^(t,.) to trail the taiget at times 
t,, (i=0, 1, , , , ) is computed as follows. The waypoint 
sequence starts from the current USV location p^ 


Pwpi^o) Pa^ ( 11 ) 

where to is the current time. A unit vector from the USV to the 
predicted position of the target at time t, (i>0) is 


Pr(fi) - Pwp(ii-i) 
llpr(ii) - Pwp(ii-i)\\ 
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where p j{t,) is the target position at time t, based on a nominal 
prediction. Due to the difference in time f. and t,._j, this unit 
vector only approximates the relative bearing of the target. 

In order to maintain detection of the target, we give the 
waypoint heading the highest priority, since the forward- 5 
looking sensor must be facing the target. Thus, we first choose 
the desired heading to be 

0,=Zk, (12) 

and then take a slice of the TD Map with a heading closest to to 
0, and time t,. Then, the “centroid of probability”, c^(6,-, t,-), is 
computed as the average of all cells in the map, weighted by 
the probability detection at each cell: 


cp(9i, U) = 

Z Pixjk, 9i, ti ) 


where x^.j. is the position at the (j, k) element of the TD Map 
grid that is sliced at heading 0, and time t, . Note that Cp is close 
to the centroid of the detectability region’s shape for the 
current sensor model, since the map is nearly uniform. Also, 25 
the center of probability is a good choice given the current 
sensor model’s convex shape. 

The next step is to check if the centroid Cp(9,., t,) is in front 
of the USV, by calculating the along-track distance d from the 
USV to the centroid 30 

d=f2i-(cp(Qi,t^)-pjpp(ti_i^)). (14) 

Finally, the trailing waypoint at t, is obtained as 

PKy(fi)=PK7=(fi-i)+max{0,rf}K,. (15) 

If d is positive (as in FIG. 8 A), it will use the centroid Cp(0„ 
t,) projected onto a line that starts from p ppp(tt_i) and is in the 
direction of n,. Otherwise (as in FIG. 8B), we skip using this 
time t, because the motion to this point p^(tj) would require 
the USV to turn away from the target, thus causing the USV 40 
to lose track of the submarine. In this case, the USV will 
“wait” for the target to pass in front of it instead. 

A series of positions p ^^(t,) with the arrival timestamp t, is 
passed to the local planner in order to adjust the desired goal 
direction and desired speed in (10). The strategic planner uses 45 
the latest TD Maps and typically updates the waypoint goal in 
a receding horizon fashion, before the USV actually reaches 
the waypoint goal. 

Set-Up 

The COLREGS algorithm was integrated within JPL’s 50 
autonomy suite called CARACaS (Control Architecture for 
Robotic Agent Command and Sensing) (see T. Huntsberger, 

H. Aghazarian, A. Castano, G. Woodward, C. Padgett, D. 
Gaines, and C. Buzzell, “Intelligent autonomy for unmanned 
sea surface and underwater vehicles,” in AUVSI Unmanned 55 
Systems North America, 2008) that can receive vehicle state 
estimates from the on-board INS unit and perception outputs 
such as hazard maps, moving contact lists from the JPL stereo 
camera server or the SIS world map server. 

CARACaS commands the USV through an integrated 60 
CAN bus. This CARACaS setup has been tested on the water 
to demonstrate various capabilities in the past including har- 
bor patrol, boat following, and static hazard avoidance (see 
M. T. Wolf, C. Assad, Y. Kuwata, A. Howard, H. Aghazarian, 

D. Zhu, T. Lu, A. Trebi-Ollermu, and T. Huntsberger, “360- 65 
degree visual detection and taiget tracking on an autonomous 
surface vehicle,” Journal of Field Robotics, vol. 27, no. 6, pp. 
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819-833, 2010; L. Elkins, D. Sellers, andW. R. Monach, “The 
autonomous maritime navigation (AMN) project: Field tests, 
autonomous and cooperative behaviors, data fusion, sensors, 
and vehicles,” Journal of Field Robotics, vol. 27, no. 6, pp. 
790-818, 2010; and T. Huntsberger, H. Aghazarian, A. 
Howard, and D. C. Trotz, “Stereo vision based navigation for 
autonomous surface vessels,” Journal of Field Robotics, vol. 
28, no. l,pp. 3-18, 2011. 

FIG. 9A shows the USV, called CMV, used in the test. 
CMV has been tested on the water, controlled with CARA- 
CaS, to demonstrate various capabilities in the past including 
harbor patrol, boat following, and static hazard avoidance. 
The traffic ships used in the tests are one 12m craft (shown in 
FIG. 9B) and two 7 m RHIBs (shown in FIG. 9C). 

FIG. 10 shows a picture of the stereo cameras. The stereo 
bar has two pairs of stereo cameras, where one pair is looking 
left, and the other pair is looking right. The current JPL stereo 
system provides the position and velocity estimates of con- 
tacts (e.g., traffic boats) but not their orientation. The heading 
estimate of the traffic boat becomes noisier when the speed of 
the traffic boat is very low, especially with the waves in the 
ocean. In one embodiment, those contacts that are moving 
slower than an empirically determined threshold are treated 
as stationary hazards and COLREGS constraints are not 
applied. 

Legends 

FIG. 11 shows a snapshot of a simulation run, involving 
one USV at the bottom and two moving vessels. The way- 
point of the USV is set in front of the vehicle start location. 
There is a traffic boat coming from the right. The USV rec- 
ognizes that it is in a crossing situation and starts maneuver- 
ing toward starboard. 

The regions around the USV represent the velocity deci- 
sion space. As shown in the lower right of the figure, there is 
a region that has a safe velocity, there is a region that repre- 
sents VO, there is a region that i s expanded by WVO, acting as 
a safety buffer, and there is a region that represents a COL- 
REGS violation (the “crossing” rule in this figure). Since the 
cost liinction is continuous, the color has some shading. There 
is a region limits the maximum velocity that the motion 
planner can choose. The line from the USV shows the best 
velocity (and its direction) selected by the algorithm. 

As shown in the upper right of FIG. 11, a small dot is 
plotted above the moving contact if it is in a COLREGS 
situation with the USV. The dot can be color-coded (or it 
could be coded with a shape or some other indicator) to 
represent the type of the COLREGS, and we consider head- 
on, overtaking, and crossing from the right and left in this 
discussion. Note that because of the hysteresis in the rule 
selection, a traffic vessel could be considered to be in more 
than one COLREGS situation, and could be marked with 
multiple dots with different colors. 

Examples 

COLREGS Runs with Four Vessels 

We now present the results of COLREGS on-water tests 
involving four vessels. The CMV was used as the USV, as 
shown in FIG. 9A, and the 1 2 m craft and two 7 m RHIBs are 
used as traffic vessels, as shown in FIG. 9B and FIG. 9C. In all 
scenarios, the USV was given a waypoint that was about 1 000 
m away in front, and was commanded to reach the waypoint 
while obeying COLREGS with traffic vessels. The USV’s 
nominal speed was 8 knots (4 m/s) in these tests. The algo- 
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rithmic parameters such as thresholds to decide which rules to 
apply were tuned through simulation and at-sea tests with 
feedback from boat drivers. 

Scenario #1 : Head-on and Crossing: 

In this scenario, the USV first overtakes a traffic vessel. As 5 
it maneuvers around it, other two vessels start to approach the 
USV, creating a head-on situation. 

FIG. 12A1 through FIG. 12F show the results of the on- 
water run. The images show the body-fixed view of the run: 
two images at the top are from the left-looking and right- to 
looking stereo cameras, and the image below them shows the 
USV, detected objects, and the decision space of the COL- 
REGS motion planner, as shown schematically in FIG. 11. 
Two-dimensional plots in the global frame in plan view are 
provided, showing the USV path, the current pose of the USV 15 
(marked with a triangle), the stereo camera field of view 
(denoted with gray lines), and the objects that were detected 
by the stereo system. 

The USV is first given a waypoint and drives toward it. In 
FIG. 12A1, the USV detects one of the 7 m RHIBs at a far 20 
distance. The estimated velocity is low enough that the USV 
simply treats it as a stationary hazard, and no COLREGS 
constraint is applied. The direction of the waypoint is repre- 
sented with a line. 

As the 7 m RHIB approaches, the USV recognizes the 25 
head-on situation and applies the COLREGS constraint, as 
shown in FIG. 12B1. The COLREGS constraints encourage 
the head-on evasive maneuver to turn right by prohibiting 
velocities to the left. The head-on situation is marked with a 
dot on top of the detected vessel. 30 

In FIG. 12C1, the USV detects another vessel approaching 
from the front, recognizes the USV is in a head-on COL- 
REGS situation with both vessels, and moves to the starboard 
side. At this moment, although the 1 2 m boat is visible in the 
image, it is not yet detected by the vehicle tracker in the stereo 35 
system. 

As it maneuvers around the two vessels, the 12 m craft 
comes from USV’s starboard side, creating a crossing situa- 
tion. In FIG. 12D1, the 12 m ship is detected, and the USV 
avoids all three vessels by further altering its path to its 40 
starboard. As the USV turns and changes its orientation, the 
heading difference between the 12 m boat and the USV 
changes from almost perpendicular to almost opposite. The 
USV treats the 12 m vehicle to be in both the crossing situa- 
tion (marked with a dot) and the head-on situation. Note that 45 
both “crossing from the right” and head-on COLREGS 
require that the USV alter its path to the starboard, and they 
correspond to the same COLREGS constraints in the USV’s 
decision space. 

FIG. 12E1 shows successful avoidance of the 12 m boat. 50 
Note that the rectangular contact shown in the lower left 
includes the detected contact and the minimum clearance 
between vessels (imposed by boat drivers). Therefore, the 
contact in the lower left plot looks closer to USV than the 1 2 
m boat in the stereo image. The USV then continues its drive 55 
toward the waypoint that was given initially, as shown in FIG. 
12F. 

The plots of the traffic vessels are generated purely based 
on the stereo system’s output and show the information that 
was available to the motion planner to make decision on 60 
USV’s maneuver. In fact, there is no inter-vehicle communi- 
cation during the run. 

Scenario #2: Overtake, Head-on, and Crossing: 

In FIG. 13 Al, the USV recognizes the overtaking situation 
and initiates a turn maneuver toward starboard, even though 65 
the straight line to the waypoint passes on the port side of the 
traffic boat. This behavior emerges from the COLREGS con- 
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straints, which requires the USV to overtake from the star- 
board side, obeying COLREGS. 

During the overtaking in FIG. 13B1, the USV detects 
another vessel in the head-on situation. The USV recognizes 
that it is in the head-on situation as well as overtaking. COL- 
REGS constraints require the USV to turn away from the 
waypoint when avoiding the head-on vessel. 

In FIG. 13C1, the USV avoids the head-on vessel. Another 
vessel coming from the right is visible in the stereo image, but 
is yet to be confimied by the vehicle tracker in the stereo 
system. 

After a successful avoidance of the head-on vessel, the 
USV is in both the head-on and crossing situation in FIG. 
13D1. It mms to its starboard to let the 12 m boat maintain its 
course, because that boat is the stand-on vessel in this situa- 
tion. 

Subsequent to the successful avoidance of the 1 2 m boat in 
FIG. 13E1, the 7 m RHIB being overtaken comes in the view 
again in FIG. 13F1. Because it is far away enough, the USV 
does not consider this situation to be overtaking. Note that the 
hazard avoidance constraint is always on, as shown in the 
cone in the plot. 

FIG. 13G shows the overall paths of the USV and detected 
vessels. Note the slight meander of the USV’s path in the last 
segment. This is caused by the inaccurate state estimate of the 
vessel that was being overtaken (the contacts shown in the 
upper left corner of the figure). When the traffic vessel first 
enters the field of view of the camera while the USV is 
mrning, the current object tracker tends to treat it as a vessel 
heading toward the USV. It is expected that one solution to 
this sensor issue is to extend the field of view of the stereo 
camera system by adding another pair of cameras pointing 
sideways, so that the USV can maintain tracking of the target 
vessel during the overtake maneuver. 

The run time of the motion planning algorithm of each 
cycle was a few milliseconds on a 32-by-128 velocity space 
grid on a PC 104 single-board microcomputer running the 
QNX operating system, available from QNX Software Sys- 
tems Limited. 1001 Farrar Road, Ottawa, Ontario, Canada, 
K2K 0B3. There are numerous vendors of PC 104 single 
board microcomputers, for example, VersaLogic Corp., 4211 
W. 11th Ave., Eugene Oreg. 97402. In addition, the PC/104 
Consortium creates, maintains and distributes specifications 
supporting the PC/ 1 04 form factor. The specifications include 
PC/104, PC/104-Plus, PCI-104, EBX and EPIC. The initial 
release of the PC/104 specification in March of 1992 was an 
open design offering the power and flexibility of an IBM 
compatible personal computer (e.g., a general purpose pro- 
grammable computer) in a size ideally suited for embedding. 
As mentioned before, the run time is approximately linear 
with respect to the number of obstacles. Even with 20 or more 
moving hazards, the algorithm continues to run well in real- 
time. 

Trailing Runs 

The architecture of the autonomy software is versatile, and 
the trailing runs were conducted on a different boat. FIG. 14A 
shows the USV, called PowerVent, used in the test. FIG. 14B 
shows the 11m RHIB, a traffic boat we used to generate 
COLREGS situations (the traffic boat was detected by the 
radar mounted on PowerVent). 

As a trailing target, a simulated submarine was used. The 
submarine simulator includes realistic dynamics of the sub- 
marine as well as the ability for a human operator to steer the 
submarine. The submarine was detected through a sonar 
simulator, which uses the pose of the actual PowerVent and 
incorporates a detailed sensor model to simulate the detec- 
tion, such as a limited field of view (FOV) and the range 
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dependency of the detection probability. The detected sub- 
marine is then passed to the world map server (WMS) (see L. 
Elkins, D. Sellers, and W. R. Monach, “The autonomous 
maritime navigation (AMN) project: Field tests, autonomous 
and cooperative behaviors, data fusion, sensors, and 
vehicles,” Journal of Field Robotics, vol. 27, no. 6, pp. 790- 
818, 2010), which then relays the contact to CARACaS. By 
the time CARACaS receives the contact from WMS, there is 
no distinction between a real submarine and a simulated 
submarine. 

Trailing a Maneuvering Target 

FIG. 15A through FIG. 15C show a scenario in which the 
submarine driver was aggressively attempting to evade the 
USV. The submarine is shown as a large circle. 

The sub attempts several quick changes of speed and direc- 
tion. FIG. 15A is a diagram that illustrates the submarine 
maneuvering to the right as the USV trails. FIG. 15B is a 
diagram that illustrates that the submarine has slowed down, 
and has turned 180 degrees and is now headed at the USV. 
FIG. 15C is a diagram that illustrates that the USV has main- 
tained trail through a 360 degree mm. Tight submarine mms 
generated a case in FIG. 8B, which place the USV in front of 
the centroid of the TD Map. However, the USV maintains the 
trail throughout. 

FIG. 16 shows a snapshot from a scenario, in which the 
USV trails the submarine as the sub goes under a buoy, turns 
around, and passes under the buoy again. The buoy was 
detected by the radar, which the USV treated as a stationary 
hazard to avoid, as the representation of the velocity space in 
the figure indicates. The waypoint-based reference velocity is 
in a region having a short time to collision with the buoy, so a 
velocity to the side is chosen instead, as indicated by the 
“post”. Once the USV is clear of the buoy, the velocity space 
changes to a safe region and the USV followed the reference 
velocity directly. During these mns, the submarine driver was 
unable to cause the USV to lose trail via maneuvers and use of 
static surface hazards. 

Trailing with Stationary Hazards and COLREGS 

FIG. 17 and FIG. 18 show COLREGS simations that were 
encountered while trailing the target. In FIG. 17, the USV 
applies COLREGS crossing while simultaneously avoiding a 
stationary hazard. The dark COLREGS velocity space con- 
straint is visible for the crossing situation, with the entire port 
side excluded because of the buoy on that side. Note the dot 
above the contact on the right and the region in the velocity 
decision space. Buoys to the port of the USV are being 
avoided simultaneously. 

In FIG. 18, the USV is in a head-on situation, and the 
COLREGS mle prohibits mming to port. At the same time, 
the velocity obstacle to the buoy excludes the option of con- 
tinuing straight, so the USV turns to starboard, as expected. 
Note the dot above the contact and the region in the velocity 
decision space. 

DEFINITIONS 

Unless otherwise explicitly recited herein, any reference to 
an electronic signal or an electromagnetic signal (or their 
equivalents) is to be understood as referring to a non-volatile 
electronic signal or a non-volatile electromagnetic signal. 

Recording the results from an operation or data acquisition, 
such as for example, recording results at a particular fre- 
quency or wavelength, is understood to mean and is defined 
herein as writing output data in a non-transitory manner to a 
storage element, to a machine-readable storage medium, orto 
a storage device. Non-transitory machine-readable storage 
media that can be used in the invention include electronic. 
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magnetic and/or optical storage media, such as magnetic 
floppy disks and hard disks; a DVD drive, a CD drive that in 
some embodiments can employ DVD disks, any of CD-ROM 
disks (i.e., read-only optical storage disks), CD-R disks (i.e., 
5 write-once, read-many optical storage disks), and CD-RW 
disks (i.e., rewriteable optical storage disks); and electronic 
storage media, such as RAM, ROM, EPROM, Compact Flash 
cards, PCMCIA cards, or alternatively SD or SDIO memory; 
and the electronic components (e.g., floppy disk drive, DVD 
to drive, CD/CD-R/CD-RW drive, or Compact Flash/PCMCIA/ 
SD adapter) that accommodate and read from and/or write to 
the storage media. Unless otherwise explicitly recited, any 
reference herein to “record” or “recording” is understood to 
refer to a non-transitory record or a non-transitory recording. 
15 As is known to those of skill in the machine-readable 
storage media arts, new media and formats for data storage 
are continually being devised, and any convenient, commer- 
cially available storage medium and corresponding read/ 
write device that may become available in the future is likely 
20 to be appropriate for use, especially if it provides any of a 
greater storage capacity, a higher access speed, a smaller size, 
and a lower cost per bit of stored information. Well known 
older machine-readable media are also available for use under 
certain conditions, such as punched paper tape or cards, mag- 
25 netic recording on tape or wire, optical or magnetic reading of 
printed characters (e.g., OCR and magnetically encoded sym- 
bols) and machine-readable symbols such as one and two 
dimensional bar codes. Recording image data for later use 
(e.g., writing an image to memory or to digital memory) can 
30 be performed to enable the use of the recorded information as 
output, as data for display to a user, or as data to be made 
available for later use. Such digital memory elements or chips 
can be standalone memory devices, or can be incorporated 
within a device of interest. “Writing output data” or “writing 
35 an image to memory” is defined herein as including writing 
transformed data to registers within a microcomputer. 

“Microcomputer” is defined herein as synonymous with 
microprocessor, microcontroller, and digital signal processor 
(“DSP”). It is understood that memory used by the micro- 
40 computer, including for example instructions for data pro- 
cessing coded as “firmware” can reside in memory physically 
inside of a microcomputer chip or in memory external to the 
microcomputer or in a combination of internal and external 
memory. Similarly, analog signals can be digitized by a stan- 
45 dalone analog to digital converter (“ADC”) or one or more 
ADCs or multiplexed ADC channels can reside within a 
microcomputer package. It is also understood that field pro- 
grammable array (“FPGA”) chips or application specific inte- 
grated circuits (“ASIC”) chips can perform microcomputer 
50 functions, either in hardware logic, software emulation of a 
microcomputer, or by a combination of the two. Apparatus 
having any of the inventive features described herein can 
operate entirely on one microcomputer or can include more 
than one microcomputer. 

55 General purpose programmable computers useful for con- 
trolling instrumentation, recording signals and analyzing sig- 
nals or data according to the present description can be any of 
a personal computer (PC), a microprocessor based computer, 
a portable computer, or other type of processing device. The 
60 general purpose programmable computer typically com- 
prises a central processing unit, a storage or memory unit that 
can record and read information and programs using 
machine-readable storage media, a communication terminal 
such as a wired communication device or a wireless commu- 
65 nication device, an output device such as a display terminal, 
and an input device such as a keyboard. The display terminal 
can be a touch screen display, in which case it can function as 
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both a display device and an input device. Different and/or 
additional input devices can be present such as a pointing 
device, such as a mouse or a joystick, and different or addi- 
tional output devices can be present such as an enunciator, for 
example a speaker, a second display, or a printer. The com- 
puter can run any one of a variety of operating systems, such 
as for example, any one of several versions of Windows, or of 
MacOS, or of UNIX, or of Linux. Computational results 
obtained in the operation of the general purpose computer can 
be stored for later use, and/or can be displayed to a user. At the 
very least, each microprocessor-based general purpose com- 
puter has registers that store the results of each computational 
step within the microprocessor, which results are then com- 
monly stored in cache memory for later use. 

Many functions of electrical and electronic apparatus can 
be implemented in hardware (for example, hard-wired logic), 
in software (for example, logic encoded in a program operat- 
ing on a general purpose processor), and in firmware (for 
example, logic encoded in a non-volatile memory that is 
invoked for operation on a processor as required). The present 
invention contemplates the substitution of one implementa- 
tion of hardware, firmware and software for another imple- 
mentation of the equivalent functionality using a different one 
of hardware, firmware and software. To the extent that an 
implementation can be represented mathematically by a 
transfer function, that is, a specified response is generated at 
an output terminal for a specific excitation applied to an input 
terminal of a “black box” exhibiting the transfer function, any 
implementation of the transfer function, including any com- 
bination of hardware, firmware and software implementa- 
tions of portions or segments of the transfer function, is con- 
templated herein, so long as at least some of the 
implementation is performed in hardware. 

Theoretical Discussion 

Although the theoretical description given herein is 
thought to be correct, the operation of the devices described 
and claimed herein does not depend upon the accuracy or 
validity of the theoretical description. That is, later theoretical 
developments that may explain the observed results on a basis 
different from the theory presented herein will not detract 
from the inventions described herein. 

Any patent, patent application, or publication identified in 
the specification is hereby incorporated by reference herein in 
its entirety. Any material, or portion thereof, that is said to be 
incorporated by reference herein, but which conflicts with 
existing definitions, statements, or other disclosure material 
explicitly set forth herein is only incorporated to the extent 
that no conflict arises between that incorporated material and 
the present disclosure material. In the event of a conflict, the 
conflict is to be resolved in favor of the present disclosure as 
the preferred disclosure. 

While the present invention has been particularly shown 
and described with reference to the preferred mode as illus- 
trated in the drawing, it will be understood by one skilled in 
the art that various changes in detail may be affected therein 
without departing from the spirit and scope of the invention as 
defined by the claims. 

What is claimed is: 

1. A system for navigating an unmanned waterborne sur- 
face vehicle safely to a desired location, comprising: 

one or more sensors configured to be mounted on an 
unmanned waterborne surface vehicle, said one or more 
sensors configured to detect the presence of one or more 
objects different from said unmanned waterborne sur- 
face vehicle in the vicinity of said one or more sensors, 
each of said one or more sensors having a control termi- 
nal configured to allow control of said sensor, and each 
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of said one or more sensors having an output terminal 
configured to provide a signal representative of said 
presence of said one or more objects different from said 
unmanned waterborne surface vehicle; and 

5 a general purpose programmable computer including 
instruction recorded on a machine readable memory, 
said general purpose programmable computer opera- 
tively coupled to each of said one or more sensors, said 
general purpose programmable computer when operat- 
ing under said instructions configured to perform the 
following steps: 

(i) controlling an operational state of each of said one or 
more sensors, said operational state selected from the 
group of operational states consisting of an on state 
and an off state; 

(ii) receiving from said one or more sensors one or more 
respective output signals representative of the pres- 
ence of said one or more objects different from said 

20 unmanned waterborne surface vehicle; 

(iii) deducing from said received one or more respective 
output signals whether there is present said one or 
more objects different from said unmanned water- 
borne surface vehicle; 

25 (iv) if there is present said one or more objects different 
from said unmanned waterborne surface vehicle, 
computing for each such object a location, a velocity 
and a heading; 

(v) applying a first set of rules configured to direct said 
unmanned waterborne surface vehicle to reach a 
desired location; 

(vi) applying a second set of rules configured to direct 
said unmanned waterborne surface vehicle to avoid 
each of said one or more objects different from said 
unmamied waterborne surface vehicle that are 
present; 

(vii) generating a command to be issued to said 
unmanned waterborne surface vehicle to operate at a 

40 particular velocity and along a particular heading, so 

as to approach said desired location while avoiding a 
collision with each of said one or more objects differ- 
ent from said unmanned waterborne surface vehicle 
that are present; and 

45 (viii) iteratively repeating steps (i) through (vii) until 
said desired location is reached. 

2. The system for navigating an unmanned waterborne 
surface vehicle safely to a desired location of claim 1, further 
comprising an umnanned waterborne surface vehicle opera- 

50 lively connected to said one or more sensors and to said 
general purpose programmable computer. 

3. The system for navigating an unmanned waterborne 
surface vehicle safely to a desired location of claim 1 , wherein 
said object different from said unmanned waterborne surface 

55 vehicle is a hazardous object. 

4 . The system for navigating an unmanned waterborne 
surface vehicle safely to a desired location of claim 1 , wherein 
said object different from said unmanned waterborne surface 
vehicle is an object of interest to be followed. 

60 5. The system for navigating an unmanned waterborne 

surface vehicle safely to a desired location of claim 1 , wherein 
said desired location is a position in proximity to said object 
of interest to be followed. 

6. The system for navigating an unmanned waterborne 

65 surface vehicle safely to a desired location of claim 5, wherein 
said desired location is updated using a most recent position 
of said object of interest to be followed. 
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7. The system for navigating an unmanned waterborne 
surface vehicle safely to a desired location of claim 1, wherein 
said desired location is a location defined on a map of the 
Earth’s surface. 

8. The system for navigating an unmanned waterborne 
surface vehicle safely to a desired location of claim 1 , wherein 
said general purpose programmable computer when operat- 
ing under said instructions configured to perform the step of 
displaying to a user information that is representative of said 
location, said velocity and said heading for at least one of said 
one or more objects different from said unmanned waterborne 
surface vehicle deduced as being present in step (iii). 

9. The system for navigating an unmanned waterborne 
surface vehicle safely to a desired location of claim 8, wherein 
said location, said velocity and said heading are computed in 
step (iv) relative to a location, a velocity and a heading of said 
unmanned waterborne surface vehicle. 

10. The system for navigating an unmanned waterborne 
surface vehicle safely to a desired location of claim 1 , wherein 
said general purpose programmable computer includes a user 
interface that allows a human operator to override a command 
generated in step (viii). 

11. The system for navigating an unmanned waterborne 
surface vehicle safely to a desired location of claim 1 , wherein 
said first set of rules is a strategic planner. 

12. The system for navigating an unmanned waterborne 
surface vehicle safely to a desired location of claim 1 , wherein 
said second set of rules is a local planner. 

13. The system for navigating an unmanned waterborne 
surface vehicle safely to a desired location of claim 1, wherein 
said second set of rules includes International Regulations for 
Preventing Collisions at Sea (COLREGS rules). 

14. The system for navigating an unmanned waterborne 
surface vehicle safely to a desired location of claim 1 , wherein 
said one or more sensors are selected from the group of 
sensors consisting of radar, sonar, lidar and stereo cameras. 

15. A method of navigating an unmanned waterborne sur- 
face vehicle safely to a desired location, comprising the steps 
of: 

(i) controlling an operational state of each of one or more 
sensors, said operational state selected from the group of 
operational states consisting of an on state and an off 
state, said one or more sensors configured to detect the 
presence of one or more objects different from said 
unmanned waterborne surface vehicle in the vicinity of 
said one or more sensors, each of said one or more 
sensors having a control terminal configured to allow 
control of said sensor, and each of said one or more 
sensors having an output terminal configured to provide 
a signal representative of said presence of said one or 
more objects different from said unmanned waterborne 
surface vehicle; and 
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using a general purpose programmable computer 
including instruction recorded on a machine readable 
memory to perform the following steps: 

(ii) receiving from said one or more sensors one or more 

5 respective output signals representative of the presence 

of said one or more objects different from said 
unmanned waterborne surface vehicle; 

(iii) deducing from said received one or more respective 
output signals whether there is present said one or more 
objects different from said unmanned waterborne sur- 
face vehicle; 

(iv) if there is present said one or more objects different 
from said unmanned waterborne surface vehicle, com- 
puting for each such object a location, a velocity and a 
heading; 

(v) applying a first set of rules configured to direct said 
umnaimed waterborne surface vehicle to reach a desired 
location; 

(vi) applying a second set of rules configured to direct said 
unmanned waterborne surface vehicle to avoid each of 

20 said one or more objects different from said unmanned 
waterborne surface vehicle that are present; 

(vii) generating a command to be issued to said unmanned 
waterborne surface vehicle to operate at a particular 
velocity and along a particular heading, so as to 

25 approach said desired location while avoiding a colli- 
sion with each of said one or more objects different from 
said umnanned waterborne surface vehicle that are 
present; and 

(viii) iteratively repeating steps (i) through (vii) until said 

30 desired location is reached by said unmanned water- 
borne surface vehicle initially located at a first known 
location. 

16. The method of navigating an unmanned waterborne 
surface vehicle safely to a desired location of claim 15, 

35 wherein said desired location is a location proximate to one of 
the objects deduced to be present in step (iii). 

17. The method of navigating an unmanned waterborne 
surface vehicle safely to a desired location of claim 15, 
wherein said desired location is a predicted location of one of 

40 the objects deduced to be present in step (iii). 

18. The method of navigating an unmanned waterborne 
surface vehicle safely to a desired location of claim 15, 
wherein said second set of rules includes International Regu- 
lations for Preventing Collisions at Sea (COLREGS rules). 

45 19. The method of navigating an unmanned waterborne 

surface vehicle safely to a desired location of claim 15, 
wherein said one or more sensors are selected from the group 
of sensors consisting of radar, sonar, lidar and stereo cameras . 

20. The method of navigating an unmanned waterborne 

50 surface vehicle safely to a desired location of claim 15, 
wherein said object different from said unmanned waterborne 
surface vehicle is an object of interest to be followed. 



